/****************************************************************************/
/*	include files
*****************************************************************************/
#include "include.h"



/****************************************************************************/
/*	Local variable  definitions
*****************************************************************************/





float InvSqrt (float x)
{
	float xhalf = 0.5f*x;
	int i = *(int*)&x;
	i = 0x5f3759df - (i>>1);
	x = *(float*)&i;
	x = x*(1.5f - xhalf*x*x);
	return x;
}



volatile float inv_value;
//volatile s32 sqrt_temp_value;
inline s32 SQRT(s32 LOC_SQRT_VALUE)
{
	s32 inv_value_Q16;
	s32 value_return;
//	sqrt_temp_value = LOC_SQRT_VALUE;
	inv_value = InvSqrt((float)LOC_SQRT_VALUE);
	inv_value_Q16 = (s32)(inv_value*2097120);
	value_return = ((2097120)/inv_value_Q16);

	return value_return;
}

/*****************************************************************************
 ** \brief	unsigned int sqrt(unsigned long value)
 **			
 ** \param [in] none
 ** \return  none
 ** \note	
*****************************************************************************/

volatile s32 SqrtValueTemp = 0;
inline s32 sqrt_Vs(s32 Vd)
{
	if((Vd)>(Foc_lib.VdLimit))	
	{
		(Vd) =  Foc_lib.VdLimit;
	}
	else if((Vd)<(-Foc_lib.VdLimit))
	{
		(Vd) = (-Foc_lib.VdLimit);
	}
	SqrtValueTemp = (Foc_lib.SvpwmVolts>>2)*(Foc_lib.SvpwmVolts>>2) - (Vd>>2)*(Vd>>2); 
	SqrtValueTemp = SQRT((s32)SqrtValueTemp);
	SqrtValueTemp = (SqrtValueTemp<<2);//vq = sqrt(vs^2-vd^2)
	return (s32)SqrtValueTemp;
}

inline s32 sqrt_Vs_high_volt(s32 Vd)
{
	if((Vd)>(Foc_lib.VdLimit))	
	{
		(Vd) =  Foc_lib.VdLimit;
	}
	else if((Vd)<(-Foc_lib.VdLimit))
	{
		(Vd) = -Foc_lib.VdLimit;
	}
	SqrtValueTemp = (Foc_lib.VsVolts>>4)*(Foc_lib.VsVolts>>4) - (Vd>>4)*(Vd>>4); 
	SqrtValueTemp = SQRT((s32)SqrtValueTemp);
	SqrtValueTemp = (SqrtValueTemp<<4);
	return (s32)SqrtValueTemp;
}



